//------------------------------------------------------------------------------
//  <copyright file="ControllerExecutionEngine.h" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#pragma once

#include "stdafx.h"
#include "ControllerStateMachineRegisters.h"
#include "ControllerStateMachineExports.h"
#include <ControllerStateMachineDescription.cs>

// The maximum number of operands that an operation can accept.
#define MAX_OPERANDS 3

// Version number
#define CEE_VERSION 1

/// <summary>
/// Executes a controller specified at runtime.  The caller is responsible for controllers 
/// which are specified in the form of 
/// a state machine, where each state has a designated control mode associated with it (e.g.
/// Do Nothing, Position Control, Impedance Control) and a set of operations that are executed
/// at each time step.  The operations are specified at runtime when the machine is created, and 
/// the operate on a set of registers, also specified at runtime.  These registers are comprised
/// of two groups, the first of which are feedback registers that describe the current state
/// of the the hardware, and the second of which are a set of working registers that are used
/// either for working memory or as control references and can be written to by an external component
/// as a result of an API call, or as the result of state operations, transition operations.
///
/// Type 'T' defines the register type (e.g. double, float, etc...) used by the state machines operations.
/// </summary>
template <class T>
class ControllerExecutionEngine
{
public:
	/// <summary> Initializes a new instance of the ControllerExecutionEngine class.  </summary>
	/// <param name="pWorkingRegisters"> The working registers to be used by the controller. </param>
	/// <param name="pFeedbackRegisterCount"> The feedback registers to be used by the controller. </param>
	/// <param name="ppOpPointers"> Pointers to the operations utilized by the state machine. </param>
	ControllerExecutionEngine(
		ControllerStateMachineRegisters<T> *pWorkingRegisters,
		ControllerStateMachineRegisters<T> *pFeedbackRegisters,
		void (**ppOpPointers)(T**),
		int version);

	/// <summary> Destructor </summary>
	~ControllerExecutionEngine();

	/// <summary>  Sets the machine description, defining states and transitions for the machine.  </summary>
	/// <param name="pCsmd"> The machine description. </param>
	void SetControllerStateMachineDescription(ControllerStateMachineDescription *pCsmd);

	/// <summary> Gets a pointer to a given register. </summary>
	/// <param name="registerID"> RegisterID object denoting the register to get. </param>
	/// <returns> A pointer to a register. </returns>
	T* GetRegister(RegisterID registerID);

	/// <summary> Returns a pointer to a working register </summary>
	/// <param name="registerIndex"> Integer indicating a register </param>
	/// <returns> A pointer to a single register values or an array of register values.  </returns>
	T* ControllerExecutionEngine<T>::GetWorkingRegister(unsigned int registerIndex);

	/// <summary> Returns a pointer to a feedback register </summary>
	/// <param name="registerIndex"> Integer indicating a register </param>
	/// <returns> A pointer to a single register values or an array of register values.  </returns>
	T* ControllerExecutionEngine<T>::GetFeedbackRegister(unsigned int registerIndex);

	/// <summary>
	/// Execute the machine until the it comes to rest in a state.  I.e. execute until 
	/// a state transitions back to itself.
	/// </summary>
	/// <returns> The state the machine came to rest in. </returns>
	int ExecuteOne();

	/// <summary> Gets a pointer to the ControllerStateMachineRegisters object containing the working registers. </summary>
	/// <returns> A pointer to the ControllerStateMachineRegisters object containing the working registers. </returns>
	ControllerStateMachineRegisters<T> *GetPointerToWorkingRegisters();

	/// <summary> Gets a pointer to the ControllerStateMachineRegisters object containing the feedback registers. </summary>
	/// <returns> A pointer to the ControllerStateMachineRegisters object containing the feedback registers. </returns>
	ControllerStateMachineRegisters<T> *GetPointerToFeedbackRegisters();

	/// <summary> Replace the pointer to the feedback registers.  </summary>
	/// <param name="pNewFeedbackRegisters"> Pointer to the new feedback registers. </param>
	void ReplaceFeedbackRegisters(ControllerStateMachineRegisters<T> *pNewFeedbackRegisters);

	/// <summary> Replace the pointer to the working registers.  </summary>
	/// <param name="pNewWorkingRegisters"> Pointer to the new working registers. </param>
	void ReplaceWorkingRegisters(ControllerStateMachineRegisters<T> *pNewWorkingRegisters);

	/// <summary>
	/// Sets the status of the machine to "Running", unless current
	/// status is "Error"
	/// </summary>
	void Start();

	/// <summary>
	/// Sets the status of the machine to "Stopped" unless current 
	/// status is "Error" or "NotStarted"
	/// </summary>
	void Stop();

	/// <summary> Gets the current status of the controller.</summary>
	/// <returns> The current status fo the controller.</returns>
	ControllerStatus GetControllerStatus();

	/// <summary> Gets the description of a controller state.  </summary>
	/// <param name="state"> State to get the description of. </param>
	/// <returns> A pointer to a controller state description. </returns>
	ControllerState* GetState(int state);

private:

	/// <summary> The current working registers </summary>
	ControllerStateMachineRegisters<T> *pWorkingRegisters;

	/// <summary> The current feedback registers. </summary>
	ControllerStateMachineRegisters<T> *pFeedbackRegisters;

	/// <summary> Pointers to the functions that execute the machines operations. </summary>
	void (**ppOpPointers)(T**); 

	/// <summary> Current status of the machine. </summary>
	ControllerStatus controllerStatus;

	/// <summary>
	/// Current machine description. Describes, states, operations
	/// transitions, etc...
	/// </summary>
	ControllerStateMachineDescription *pCsmd;

	/// <summary> State number that the state machine is currently in. </summary>
	int currentState;

	/// <summary> Build an array of register pointers for an operations parameters </summary>
	/// <param name="pOperation"> Description of the operation and operands </param>
	/// <returns> An array of register pointers. </returns>
	T** SetParameters(ControllerOperation *pOperation);

	/// <summary> 
	/// Build an array of register pointers for an operations parameters. Includes
	/// a specific register for returning a single double.  Used to when checking for
	/// transitions.
	/// </summary>
	/// <param name="pTransition"> Description of the transition, operations, and operands. </param>
	/// <param name="pResultRegister"> Destination of result.  Should point to a single value.  </param>
	/// <returns> An array of register pointers. </returns>
	T** SetParameters(ControllerTransition *pTransition, T *pResultRegister);

	/// <summary> Execute the operations in a list.  </summary>
	/// <param name="pOperations"> A list of operations. </param>
	void ExecuteOperations(OperationList *pOperations);

	/// <summary> Execute a single operation </summary>
	/// <param name="pOperation"> The operation to execute </param>
	void ExecuteOperation(ControllerOperation *pOperation);

	/// <summary> Execute transition test operation </summary>
	/// <param name="pTransition"> Transition description </param>
	/// <param name="pResultRegister"> Location to store result of test </param>
	void ExecuteTransitionTestOperation(ControllerTransition *pTransition, T *pResultRegister);

	/// <summary> Evaluate transition conditions.  Executing the transition if appropriate.  </summary>
	/// <param name="state"> A controller state. </param>
	/// <returns> True if a transition took palce. </returns>
	bool EvaluateTransitions(ControllerState *pState);
};

/// <summary> Initializes a new instance of the ControllerExecutionEngine class.  </summary>
/// <param name="pWorkingRegisters"> The working registers to be used by the controller. </param>
/// <param name="pFeedbackRegisterCount"> The feedback registers to be used by the controller. </param>
/// <param name="ppOpPointers"> Pointers to the operations utilized by the state machine. </param>
template <class T> 
ControllerExecutionEngine<T>::ControllerExecutionEngine(
	ControllerStateMachineRegisters<T> *pWorkingRegisters,
	ControllerStateMachineRegisters<T> *pFeedbackRegisters,
	void (*ppOpPointers [])(T**),
	int version)
{
	if (version != CEE_VERSION)
	{
		throw "Version mismatch";
	}

	this->ppOpPointers = ppOpPointers;
	this->pWorkingRegisters = pWorkingRegisters;
	this->pFeedbackRegisters = pFeedbackRegisters;

	this->pCsmd = new ControllerStateMachineDescription();
	this->controllerStatus = ControllerStatus::Error;
}

template <class T> 
ControllerExecutionEngine<T>::~ControllerExecutionEngine()
{
	delete this->pCsmd;
}

/// <summary>  Sets the machine description, defining states and transitions for the machine.  </summary>
/// <param name="pCsmd"> The machine description. </param>
template <class T>
void ControllerExecutionEngine<T>::SetControllerStateMachineDescription(ControllerStateMachineDescription *pCsmd)
{
	memcpy_s(
		this->pCsmd,
		sizeof(ControllerStateMachineDescription),
		pCsmd,
		sizeof(ControllerStateMachineDescription));

		this->controllerStatus = ControllerStatus::NotStarted;

	this->currentState = 0;
}

/// <summary>
/// Execute the machine until the it comes to rest in a state.  I.e. execute until 
/// a state transitions back to itself.
/// </summary>
/// <returns>
/// The state the machine came to rest in. -1 returned if no valid controller
/// state machine is currently being run.
/// </returns>
template <class T>
int ControllerExecutionEngine<T>::ExecuteOne()
{
	if(this->pCsmd == NULL)
	{
		return -1;
	}

	if(this->controllerStatus == ControllerStatus::NotStarted)
	{
		this->controllerStatus = ControllerStatus::Running;
	}

	if(this->controllerStatus != ControllerStatus::Running)
	{
		return -1;
	}

	// execute operations
	ControllerState *pCurrentControllerState;

	do
	{
		pCurrentControllerState = this->GetState(this->currentState);

		this->ExecuteOperations(&(pCurrentControllerState->Operations));

	}
	while (this->EvaluateTransitions(pCurrentControllerState));

	return this->currentState;
}

/// <summary> Build an array of register pointers for an operation's parameters </summary>
/// <param name="operation"> Description of the operation and operands. </param>
/// <returns> An array of register pointers. </returns>
template <class T>
T **ControllerExecutionEngine<T>::SetParameters(ControllerOperation *pOperation)
{
	T **ppParameters;

	if(pOperation->Result.RegisterType != RegisterTypes::Working)
	{
		throw "Results must be placed in machine registers";
	}

	int numOperands = pOperation->NumOperands;

	ppParameters = new T*[numOperands + 1];

	ppParameters[0] = this->GetRegister(pOperation->Result);
	
	if(numOperands > 0)
	{
		ppParameters[1] = this->GetRegister(pOperation->Operand0);
	}

	if(numOperands > 1)
	{
		ppParameters[2] = this->GetRegister(pOperation->Operand1);
	}

	if(numOperands > 2)
	{
		ppParameters[3] = this->GetRegister(pOperation->Operand2);
	}

	if(numOperands > 3)
	{
		ppParameters[4] = this->GetRegister(pOperation->Operand3);
	}

	if(numOperands > 4)
	{
		ppParameters[5] = this->GetRegister(pOperation->Operand4);
	}

	if(numOperands > 5)
	{
		ppParameters[6] = this->GetRegister(pOperation->Operand5);
	}
	return ppParameters;
}

/// <summary> 
/// Build an array of register pointers for an operations parameters. Includes
/// a specific register for returning a single double.  Used to when checking for
/// transitions.
/// </summary>
/// <param name="operation"> Description of the operation and operands. </param>
/// <param name="resultReg"> Destination of result.  Should be a one double.  </param>
/// <returns> An array of register pointers. </returns>
template <class T>
T** ControllerExecutionEngine<T>::SetParameters(ControllerTransition *pTransition, T *pResultRegister)
{
	T **ppParameters;

	int numOperands = pTransition->NumTestOperands;

	ppParameters = new T*[numOperands + 1];

	ppParameters[0] = pResultRegister;
	
	if(numOperands > 0)
	{
		ppParameters[1] = this->GetRegister(pTransition->TestOperand0);
	}

	if(numOperands > 1)
	{
		ppParameters[2] = this->GetRegister(pTransition->TestOperand1);
	}

	if(numOperands > 2)
	{
		ppParameters[3] = this->GetRegister(pTransition->TestOperand2);
	}

	return ppParameters;
}

/// <summary> Execute transition test operation </summary>
/// <param name="pTransition"> Transition description </param>
/// <param name="pResultRegister"> Location to store result of test </param>
template <class T>
void ControllerExecutionEngine<T>::ExecuteTransitionTestOperation(ControllerTransition* pTransition, T *pResultRegister)
{
	T** ppParameters = SetParameters(pTransition, pResultRegister);
	this->ppOpPointers[pTransition->TestOpCode](ppParameters);
	delete ppParameters;
}

/// <summary> Evaluate transition conditions.  Executing the transition if appropriate.  </summary>
/// <param name="pState"> A controller state. </param>
/// <returns> True if a transition took palce. </returns>
template <class T>
bool ControllerExecutionEngine<T>::EvaluateTransitions(ControllerState *pState)
{
	bool transitionOccured = false;
	T *pResultReg = new T();
	if(pState->NumTransitions > 0)
	{	
		this->ExecuteTransitionTestOperation(&(pState->Transition0), pResultReg);
		if(*pResultReg != 0)
		{
			this->ExecuteOperations(&(pState->Transition0.Operations));
			this->currentState = pState->Transition0.TargetState;
			transitionOccured = true;
		}
	}

	if(pState->NumTransitions > 1 && !transitionOccured)
	{	
		this->ExecuteTransitionTestOperation(&(pState->Transition1), pResultReg);
		if(*pResultReg != 0)
		{
			this->ExecuteOperations(&(pState->Transition1.Operations));
			this->currentState = pState->Transition1.TargetState;
			transitionOccured = true;
		}
	}

	if(pState->NumTransitions > 2 && !transitionOccured)
	{	
		this->ExecuteTransitionTestOperation(&(pState->Transition2), pResultReg);
		if(*pResultReg != 0)
		{
			this->ExecuteOperations(&(pState->Transition2.Operations));
			this->currentState = pState->Transition2.TargetState;
			transitionOccured = true;
		}
	}

	delete pResultReg;
	return transitionOccured;
}

/// <summary> Execute a single operation </summary>
/// <param name="pOperation"> The operation to execute </param>
template <class T>
void ControllerExecutionEngine<T>::ExecuteOperation(ControllerOperation *pOperation)
{
	T **ppParameters = this->SetParameters(pOperation);
	this->ppOpPointers[pOperation->OpCode](ppParameters);
	delete ppParameters;
}

/// <summary> Execute the operations in a list.  </summary>
/// <param name="pOperations"> A list of operations. </param>
template <class T>
void ControllerExecutionEngine<T>::ExecuteOperations(OperationList *pOperations)
{
	if(pOperations->NumOperations > 0 )
	{
		this->ExecuteOperation(&(pOperations->Operation0));
	}

	if(pOperations->NumOperations > 1 )
	{
		this->ExecuteOperation(&(pOperations->Operation1));
	}

	if(pOperations->NumOperations > 2 )
	{
		this->ExecuteOperation(&(pOperations->Operation2));
	}
}

/// <summary> Gets the description of a controller state.  </summary>
/// <param name="state"> State to get the description of. </param>
/// <returns> A controller state description. </returns>
template <class T>
ControllerState* ControllerExecutionEngine<T>::GetState(int state)
{
	if(state >= this->pCsmd->NumStates || state < 0)
	{
		this->controllerStatus = ControllerStatus::Error;
	}

	if(state == 0)
	{
		return &(this->pCsmd->State0);
	}
	else if(state == 1)
	{
		return &(this->pCsmd->State1);
	}
	else if(state == 2)
	{
		return &(this->pCsmd->State2);
	}

	return NULL;
}

/// <summary> Returns a pointer to a register </summary>
/// <param name="registerID"> Indicates type and index of register</param>
/// <returns> A pointer to a single double or an array of doubles.  </returns>
template <class T>
T* ControllerExecutionEngine<T>::GetRegister(RegisterID registerID)
{
	if(registerID.RegisterType == RegisterTypes::Working)
	{
		return this->pWorkingRegisters->GetRegister(registerID.RegisterNumber);
	}
	else if (registerID.RegisterType == RegisterTypes::Feedback)
	{
		return this->pFeedbackRegisters->GetRegister(registerID.RegisterNumber);
	}
	else
	{
		throw "Unknown register type.";
	}
}

/// <summary> Returns a pointer to a working register </summary>
/// <param name="registerIndex"> Integer indicating a register </param>
/// <returns> A pointer to a single register values or an array of register values.  </returns>
template <class T>
T* ControllerExecutionEngine<T>::GetWorkingRegister(unsigned int registerIndex)
{
	return this->pWorkingRegisters->GetRegister(registerIndex);
}

/// <summary> Returns a pointer to a feedback register </summary>
/// <param name="registerIndex"> Integer indicating a register </param>
/// <returns> A pointer to a single register values or an array of register values.  </returns>
template <class T>
T* ControllerExecutionEngine<T>::GetFeedbackRegister(unsigned int registerIndex)
{
	return this->pFeedbackRegisters->GetRegister(registerIndex);
}

/// <summary> Replace the pointer to the feedback registers.  </summary>
/// <param name="pNewFeedbackRegisters"> Pointer to the new feedback registers. </param>
template <class T>
void ControllerExecutionEngine<T>::ReplaceFeedbackRegisters(ControllerStateMachineRegisters<T> *pNewFeedbackRegisters)
{
	this->pFeedbackRegisters = pNewFeedbackRegisters;
}

/// <summary> Replace the pointer to the working registers.  </summary>
/// <param name="pNewWorkingRegisters"> Pointer to the new working registers. </param>
template <class T>
void ControllerExecutionEngine<T>::ReplaceWorkingRegisters(ControllerStateMachineRegisters<T> *pNewWorkingRegisters)
{
	this->pWorkingRegisters = pNewWorkingRegisters;
}

/// <summary> Gets a pointer to the working registers. </summary>
/// <returns> A pointer to the working registers. </returns>
template <class T>
ControllerStateMachineRegisters<T> *ControllerExecutionEngine<T>::GetPointerToWorkingRegisters()
{
	return this->pWorkingRegisters;
}

/// <summary> Gets a pointer to the feedback registers. </summary>
/// <returns> A pointer to the feedback registers. </returns>
template <class T>
ControllerStateMachineRegisters<T> *ControllerExecutionEngine<T>::GetPointerToFeedbackRegisters()
{
	return this->pFeedbackRegisters;
}

/// <summary>
/// Sets the status of the machine to "Running", unless current
/// status is "Error"
/// </summary>
template <class T>
void ControllerExecutionEngine<T>::Start()
{
	if(this->controllerStatus != ControllerStatus::Error)
	{
		this->controllerStatus = ControllerStatus::Running;
	}
}

/// <summary>
/// Sets the status of the machine to "Stopped" unless current 
/// status is "Error" or "NotStarted"
/// </summary>
template <class T>
void ControllerExecutionEngine<T>::Stop()
{
	if( this->controllerStatus != ControllerStatus::Error &&
		this->controllerStatus != ControllerStatus::NotStarted)
	{
		this->controllerStatus = ControllerStatus::Stopped;
	}
}

/// <summary> Gets the current status of the controller.</summary>
/// <returns> The current status fo the controller.</returns>
template <class T>
ControllerStatus ControllerExecutionEngine<T>::GetControllerStatus()
{
	return this->controllerStatus;
}
